#include "motor_identify.h"
#include <stdlib.h>  // 提供 abs() 函数

ResistanceIdentifyCtrl g_res_identify;
StepInductanceIdentifyCtrl g_step_inductance_identify;
HighFreqInjectionCtrl g_hf_inj;
FluxIdentifyCtrl g_flux_identify;

//全局滤波器实例
static BiquadFilter i_d_bpf, i_q_bpf;

// ------------------- 公共初始化函数 -------------------
void MotorIdentifyInit(void)
{
    // 电阻辨识初始化
    g_res_identify.state = RESISTANCE_IDLE;
    g_res_identify.start_flag = false;
    g_res_identify.rated_current = 2.0f;    // 示例额定电流，需修改
    g_res_identify.id_ref[0] = 0.5f * g_res_identify.rated_current;
    g_res_identify.id_ref[1] = 0.8f * g_res_identify.rated_current;
		g_res_identify.sample_cnt = 0;
		g_res_identify.stable_cnt = 0;
		g_res_identify.r_identified = 0.0f;
		g_res_identify.i_d[0] = g_res_identify.i_d[1] = 0.0f;
		g_res_identify.u_d[0] = g_res_identify.u_d[1] = 0.0f;
	
	
	  g_step_inductance_identify.state = STEP_INDUCTANCE_IDLE,
    g_step_inductance_identify.start_flag = false,
    g_step_inductance_identify.step_voltage = 1.0f,
    g_step_inductance_identify.rated_current = 2.0f,
    g_step_inductance_identify.sample_freq = 10000.0f,
    g_step_inductance_identify.sample_idx = 0,
    g_step_inductance_identify.L_identified = 0.0f,

    g_step_inductance_identify.I_inf = 0.0f,
    g_step_inductance_identify.tau = 0.0f,
    g_step_inductance_identify.capture_cnt = 0,
		
		g_step_inductance_identify.ud_step = 1.0f,
		g_step_inductance_identify.Ld_identified = 0.0f;
		
		for (uint16_t i = 0; i<512 ;i++){
			g_step_inductance_identify.i_sample_buf[i] = 0.0f;
    }
		
    // 高频电感辨识初始化
    g_hf_inj.state = HF_IDLE;
    g_hf_inj.start_flag = false;
    g_hf_inj.Vh = 0.5f;          // 高频电压幅值
    g_hf_inj.wh = 2 * PI * 1000;  // 提高注入频率到1kHz（避免与电流环带宽重叠）
    g_hf_inj.Ts = 1.0f / 10000;  // 10kHz采样周期
    g_hf_inj.inj_idx = 0;
    g_hf_inj.cap_idx = 0;
    g_hf_inj.L_identified = 0.0f;
    
    for (uint16_t i = 0; i<256 ;i++){
        g_hf_inj.i_dh_buf[i] = 0.0f;
        g_hf_inj.i_qh_buf[i] = 0.0f;
    }

		
		g_flux_identify.state = FLUX_IDLE;
    g_flux_identify.start_flag = false;
    g_flux_identify.psi_f = 0.0f;
    g_flux_identify.u_q1 = 0.0f;
    g_flux_identify.u_q2 = 0.0f;
    g_flux_identify.stable_cnt = 0;
		g_flux_identify.i_d1 = 0.5f;
		g_flux_identify.i_d2 = 0.8f;
		//500Hz中心频率，10kHz采样率， Q=0.707
    InitBiquadFilter(&i_d_bpf, 1000.0f, 10000.0f, 0.707f);
    InitBiquadFilter(&i_q_bpf, 1000.0f, 10000.0f, 0.707f);
		
		
		
}



// ------------------- 电阻辨识核心逻辑 -------------------
void ResistanceIdentifyProcess(void)
{
    const uint32_t STABLE_THRESHOLD = 100; // 电流稳定判定次数
    const uint32_t SAMPLE_NUM = 100;       // 均值滤波次数

    switch (g_res_identify.state)
    {
        case RESISTANCE_IDLE:
            if (g_res_identify.start_flag) {
                g_res_identify.state = RESISTANCE_STAGE1;
                _rt_U.idq_tar_A[0] = g_res_identify.id_ref[0];
                _rt_U.idq_tar_A[1] = 0.0f;
                g_res_identify.sample_cnt = 0;
                g_res_identify.stable_cnt = 0;
            }
            break;

        case RESISTANCE_STAGE1:
        {
            float i_d = _rt_Y.idq_now_A[0]; // d轴电流反馈
            float u_d = kMotorDriver.udq_tar_V[0];               // d轴电压反馈

            // 均值滤波
            g_res_identify.i_d[0] = g_res_identify.i_d[0] * 0.9f + i_d * 0.1f;
            g_res_identify.u_d[0] = g_res_identify.u_d[0] * 0.9f + u_d * 0.1f;

            // 电流稳定判定
            if (fabs(g_res_identify.i_d[0] - g_res_identify.id_ref[0]) < 0.05f * g_res_identify.rated_current) {
                g_res_identify.stable_cnt++;
            } else {
                g_res_identify.stable_cnt = 0;
            }

            if (g_res_identify.stable_cnt > STABLE_THRESHOLD && ++g_res_identify.sample_cnt > SAMPLE_NUM) {
                g_res_identify.state = RESISTANCE_STAGE2;
                _rt_U.idq_tar_A[0] = g_res_identify.id_ref[1];
                g_res_identify.sample_cnt = 0;
                g_res_identify.stable_cnt = 0;
            }
        }
        break;

        case RESISTANCE_STAGE2:
        {
            float i_d = _rt_Y.idq_now_A[0];
						float u_d = kMotorDriver.udq_tar_V[0];               // d轴电压反馈

            g_res_identify.i_d[1] = g_res_identify.i_d[1] * 0.9f + i_d * 0.1f;
            g_res_identify.u_d[1] = g_res_identify.u_d[1] * 0.9f + u_d * 0.1f;

            if (fabs(g_res_identify.i_d[1] - g_res_identify.id_ref[1]) < 0.05f * g_res_identify.rated_current) {
                g_res_identify.stable_cnt++;
            } else {
                g_res_identify.stable_cnt = 0;
            }

            if (g_res_identify.stable_cnt > STABLE_THRESHOLD && ++g_res_identify.sample_cnt > SAMPLE_NUM) {
                g_res_identify.state = RESISTANCE_CALCULATE;
            }
        }
        break;

        case RESISTANCE_CALCULATE:
            if (fabs(g_res_identify.i_d[1] - g_res_identify.i_d[0]) > 1e-5f) {
                g_res_identify.r_identified = (g_res_identify.u_d[1] - g_res_identify.u_d[0]) 
                                           / (g_res_identify.i_d[1] - g_res_identify.i_d[0]);
            } else {
                g_res_identify.r_identified = 0.0f;
            }
            _rt_U.idq_tar_A[0] = 0.0f;
            _rt_U.idq_tar_A[1] = 0.0f;
            g_res_identify.state = RESISTANCE_FINISH;
            g_res_identify.start_flag = false;
            break;

        case RESISTANCE_FINISH:
            // 可在此处添加电阻结果处理（如存储/打印）
            g_res_identify.state = RESISTANCE_IDLE;
            break;
    }
}

// ------------------- 阶跃电感辨识核心逻辑 -------------------

// 主流程处理函数
void StepInductanceIdentifyProcess(void) {
    // 若未启动辨识，直接返回
    if (!g_step_inductance_identify.start_flag) {
        return;
    }

    // 状态机主逻辑
    switch (g_step_inductance_identify.state) {
        case STEP_INDUCTANCE_IDLE:
					if(g_step_inductance_identify.start_flag){
                if (g_res_identify.state != RESISTANCE_FINISH) {
                    ResistanceIdentifyStart();
                    g_step_inductance_identify.start_flag = false;
                    return;
                }
                
                g_step_inductance_identify.state = STEP_INDUCTANCE_D_INJECT;
                g_step_inductance_identify.sample_idx = 0;
                g_step_inductance_identify.capture_cnt = 0;
					}
            // 等待启动信号，不执行具体操作
            break;

        case STEP_INDUCTANCE_D_INJECT: {
            // 在α轴注入阶跃电压，β轴保持零
            			kMotorDriver.udq_tar_V[0] = g_step_inductance_identify.ud_step;
           				kMotorDriver.udq_tar_V[1] = 0.0f;
            
            // 计算理论稳态电流
            g_step_inductance_identify.I_inf = g_step_inductance_identify.ud_step / g_res_identify.r_identified;
            
            g_step_inductance_identify.state = STEP_INDUCTANCE_D_CAPTURE;
            break;
        }

        case STEP_INDUCTANCE_D_CAPTURE: {
            // 采集α轴电流
            float current_sample = _rt_Y.idq_now_A[0];
            
            // 存储采样数据
            
            if (g_step_inductance_identify.sample_idx < 512) {
                g_step_inductance_identify.i_sample_buf[g_step_inductance_identify.sample_idx++] = current_sample;
            }
            
            // 判断是否采集足够数据
            if (g_step_inductance_identify.capture_cnt++ >= 
                (uint32_t)(5.0 * g_step_inductance_identify.sample_freq)) {
                g_step_inductance_identify.state = STEP_INDUCTANCE_CALCULATE_D;
            }
            break;
        }

        case STEP_INDUCTANCE_CALCULATE_D: {
            // 目标电流值（63.2%稳态电流对应的时间点为τ）
            float I_target = 0.632f * g_step_inductance_identify.I_inf;
            bool found = false;
            
            // 寻找电流达到目标值的时间点
            for (uint16_t i = 0; i < g_step_inductance_identify.sample_idx; i++) {
                if (g_step_inductance_identify.i_sample_buf[i] >= I_target) {
                    // 计算时间常数 τ = 达到63.2%稳态电流的时间
                    g_step_inductance_identify.tau = (float)i / g_step_inductance_identify.sample_freq;
                    found = true;
                    break;
                }
            }
            
            // 计算电感值 L = R × τ
            if (found) {
                g_step_inductance_identify.L_identified = g_res_identify.r_identified * g_step_inductance_identify.tau;
//                COMM.L = g_step_inductance_identify.L_identified; // 更新全局电感值
                
//                printf("电感计算完成: L = %gH (R = %gΩ, τ = %gs)\n", 
//                       COMM.L, g_res_identify.r_identified, g_step_inductance_identify.tau);
            } else {
//                printf("警告: 未找到达到目标电流的时间点，电感计算失败\n");
                // 使用替代方法：基于采样数据的斜率计算
                float delta_i = g_step_inductance_identify.i_sample_buf[g_step_inductance_identify.sample_idx-1] - 
                                g_step_inductance_identify.i_sample_buf[0];
                float delta_t = (float)g_step_inductance_identify.sample_idx / g_step_inductance_identify.sample_freq;
                
                if (delta_t > 0 && fabs(delta_i) > 1e-6) {
                    g_step_inductance_identify.Ld_identified = g_step_inductance_identify.ud_step / (delta_i / delta_t);
                } else {
                    // 计算失败，重置状态
                    g_step_inductance_identify.state = STEP_INDUCTANCE_IDLE;
                    g_step_inductance_identify.start_flag = false;
                    return;
                }
            }
            
            // 恢复电压为零
         		kMotorDriver.udq_tar_V[0] = 0.0f;
            
            // 表贴式电机Ld = Lq，直接使用Ld作为总电感
            g_step_inductance_identify.L_identified = g_step_inductance_identify.Ld_identified;
            g_step_inductance_identify.state = STEP_INDUCTANCE_FINISH;
            break;
        }

        case STEP_INDUCTANCE_FINISH: {
            // 关闭启动标志
            g_step_inductance_identify.start_flag = false;
            
//            printf("电感辨识完成，最终值: L = %gH\n", COMM.L);
            return; // 返回ok以触发状态机转移
        }

        default:
            // 错误处理
            g_step_inductance_identify.state = STEP_INDUCTANCE_IDLE;
            g_step_inductance_identify.start_flag = false;
//            printf("电感辨识错误，状态重置\n");
            break;
    }

}

// ------------------- 高频电感辨识核心逻辑 -------------------
void HFVoltageInjectionGen(void)
{
    static float theta_h = 0.0f;
    g_hf_inj.u_dh = g_hf_inj.Vh * cos(theta_h);
    g_hf_inj.u_qh = g_hf_inj.Vh * sin(theta_h);
    theta_h += g_hf_inj.wh * g_hf_inj.Ts;
    if (theta_h > 2 * PI) theta_h -= 2 * PI;
}





void HFResponseExtract(float i_d, float i_q)
{
    // 1. 限幅滤波（保留原滤波逻辑，抑制噪声）
    float i_d_limited = LimitValue(i_d, -1000.0f, 1000.0f);
    float i_q_limited = LimitValue(i_q, -1000.0f, 1000.0f);

    // 2. 带通滤波器提取高频电流
    float i_d_bp = ApplyBiquadFilter(&i_d_bpf, i_d_limited);
    float i_q_bp = ApplyBiquadFilter(&i_q_bpf, i_q_limited);

    // 3. 缓存高频电流响应（用于后续幅值计算）
    if (g_hf_inj.cap_idx < 256) {
        g_hf_inj.i_dh_buf[g_hf_inj.cap_idx] = i_d_bp;
        g_hf_inj.i_qh_buf[g_hf_inj.cap_idx] = i_q_bp;
        g_hf_inj.cap_idx++;
    }

    // 4. 实时更新滤波后电流（可选：用于调试或观测）
    g_hf_inj.i_d_bp = i_d_bp;
    g_hf_inj.i_q_bp = i_q_bp;
}

// 使用DFT计算高频电流基波幅值（替代简单绝对值平均）
float CalculateHFFundamental(float *buf, uint16_t len, float wh, float Ts)
{
    float sum_cos = 0.0f, sum_sin = 0.0f;
    for (uint16_t i = 0; i < len; i++) {
        float theta = wh * i * Ts;
        sum_cos += buf[i] * cos(theta);
        sum_sin += buf[i] * sin(theta);
    }
    return sqrtf(sum_cos*sum_cos + sum_sin*sum_sin) / (len/2); // 计算DFT幅值
}

float I_hf = 0.0f;
void HighFreqInductanceProcess(void)
{
    // 定义高频注入周期数
    const uint32_t HF_INJECTION_CYCLES = 10; // 注入10个完整周期
    switch (g_hf_inj.state)
    {
			
			        case HF_IDLE:
            if (g_hf_inj.start_flag) {
                // 检查电机是否处于合适的运行状态（电角速度足够大）
                if (abs(Encoder.ElectricalSpeed) < 5.0f) { // 阈值可调整
                    // 电机转速过低，可能导致辨识不准确
                    g_hf_inj.start_flag = false;
                    return;
                }
                
                g_hf_inj.inj_idx = 0;
                g_hf_inj.cap_idx = 0;
                g_hf_inj.state = HF_INJECTION;
            }
            break;
//        case HF_IDLE:
//            if (g_hf_inj.start_flag) {
//                g_hf_inj.inj_idx = 0;
//								g_hf_inj.cap_idx = 0;
//                g_hf_inj.state = HF_INJECTION;
//            }
//            break;

        case HF_INJECTION:
            HFVoltageInjectionGen();

				kMotorDriver.udq_tar_V[0] += g_hf_inj.u_dh;//注释
				kMotorDriver.udq_tar_V[1] += g_hf_inj.u_qh;//注释

				            // 计算已注入的完整周期数
        uint32_t cycles = (uint32_t)(g_hf_inj.inj_idx * g_hf_inj.Ts * g_hf_inj.wh / (2.0f * PI));
				
						g_hf_inj.inj_idx++;
            if (g_hf_inj.inj_idx >= HF_INJECTION_CYCLES) {
                g_hf_inj.state = HF_RESPONSE_CAPTURE;
								g_hf_inj.cap_idx = 0;
							
            }
            break;

        case HF_RESPONSE_CAPTURE:
					//提取高频电流响应
           HFResponseExtract(_rt_Y.idq_now_A[0], _rt_Y.idq_now_A[1]);//注释

//            HFResponseExtract(_rt_Y.i_d_feedback, _rt_Y.i_q_feedback);
            if (g_hf_inj.cap_idx >= 256) {
                g_hf_inj.state = HF_INDUCTANCE_CALC;
            }
            break;

         case HF_INDUCTANCE_CALC:
            // 计算高频电流幅值（简化：表贴式无需区分正/负序）
            // 使用DFT计算高频电流幅值（更准确）
             I_hf = CalculateHFFundamental(g_hf_inj.i_dh_buf, 256, g_hf_inj.wh, g_hf_inj.Ts);
//            for (uint16_t i = 0; i < 256; i++) {
//                I_hf += fabs(g_hf_inj.i_dh_buf[i]); // 或用 RMS 计算
//            }
//            I_hf /= 256.0f; // 平均幅值

            // 理论公式：L_s = V_h / [ (ω_h - ω_r) * I_hf ]
            // ω_r 为电机电角速度，需从编码器获取
            float omega_diff = g_hf_inj.wh - Encoder.ElectricalSpeed;
//            if (fabs(omega_diff) > 1e-5f) 
						if (fabs(omega_diff) > 1e-5f && I_hf > 1e-5f)
							
						{
                g_hf_inj.L_identified = g_hf_inj.Vh / (omega_diff * I_hf) * 100000000;
            } else {
                g_hf_inj.L_identified = 0.0f; // 避免除以0
            }

            g_hf_inj.state = HF_IDLE;
            g_hf_inj.start_flag = false;
            break;
    }
}



// 磁链辨识主流程
void FluxIdentifyProcess(void) {
    // 从电机驱动或编码器获取电角速度
    g_flux_identify.we = Encoder.ElectricalSpeed; 

    switch (g_flux_identify.state) {
        case FLUX_IDLE:
            if (g_flux_identify.start_flag) {
                // 进入阶段1：设置第1组d轴电流，q轴电流置0
                _rt_U.idq_tar_A[0] = g_flux_identify.i_d1; 
                _rt_U.idq_tar_A[1] = 0.0f; 
                g_flux_identify.state = FLUX_STAGE1;
                g_flux_identify.stable_cnt = 0;
            }
            break;

        // 阶段1：稳定在 i_d1，采集 u_q1
        case FLUX_STAGE1: {
            float i_d_feedback = _rt_Y.idq_now_A[0]; // d轴电流反馈
            float i_q_feedback = _rt_Y.idq_now_A[1]; // q轴电流反馈

            // 判定条件：q轴电流接近0，d轴电流稳定
            if (fabs(i_q_feedback) < 0.05f &&    // q轴电流接近0
                fabs(i_d_feedback - g_flux_identify.i_d1) < 0.05f * g_flux_identify.i_d1) { // d轴电流稳定
                g_flux_identify.stable_cnt++;
            } else {
                g_flux_identify.stable_cnt = 0;
            }

            // 稳态判定：稳定一定次数后，记录q轴电压
            if (g_flux_identify.stable_cnt > g_flux_identify.STABLE_THRESHOLD) {
                g_flux_identify.u_q1 = kMotorDriver.udq_tar_V[1]; // 采集q轴电压
                // 进入阶段2：设置第2组d轴电流
                _rt_U.idq_tar_A[0] = g_flux_identify.i_d2; 
                _rt_U.idq_tar_A[1] = 0.0f; 
                g_flux_identify.state = FLUX_STAGE2;
                g_flux_identify.stable_cnt = 0;
            }
        } break;

        // 阶段2：稳定在 i_d2，采集 u_q2
        case FLUX_STAGE2: {
            float i_d_feedback = _rt_Y.idq_now_A[0];
            float i_q_feedback = _rt_Y.idq_now_A[1];

            if (fabs(i_q_feedback) < 0.05f && 
                fabs(i_d_feedback - g_flux_identify.i_d2) < 0.05f * g_flux_identify.i_d2) {
                g_flux_identify.stable_cnt++;
            } else {
                g_flux_identify.stable_cnt = 0;
            }

            if (g_flux_identify.stable_cnt > g_flux_identify.STABLE_THRESHOLD) {
                g_flux_identify.u_q2 = kMotorDriver.udq_tar_V[1]; // 采集q轴电压
                g_flux_identify.state = FLUX_CALCULATE;
            }
        } break;

        // 计算磁链：完全按照文档公式 (4-6)
        case FLUX_CALCULATE: {
            // 公式：ψ_f = (u_q1*i_d2 - u_q2*i_d1) / [we*(i_d2 - i_d1)]
            if (fabs(g_flux_identify.i_d2 - g_flux_identify.i_d1) > 1e-5f && 
                fabs(g_flux_identify.we) > 1e-5f) { // 避免除以0
                float L_s = g_hf_inj.L_identified;
									
							  g_flux_identify.psi_f = (g_flux_identify.u_q1 * g_flux_identify.i_d2 - 
                                        g_flux_identify.u_q2 * g_flux_identify.i_d1) / 
                                       (g_flux_identify.we * (g_flux_identify.i_d2 - g_flux_identify.i_d1))
                                       - L_s * (g_flux_identify.i_d2 - g_flux_identify.i_d1);
            } else {
                g_flux_identify.psi_f = 0.0f; // 异常情况置0
            }

            // 清除电流指令
            _rt_U.idq_tar_A[0] = 0.0f; 
            _rt_U.idq_tar_A[1] = 0.0f; 
            g_flux_identify.state = FLUX_FINISH;
            g_flux_identify.start_flag = false;
        } break;

        case FLUX_FINISH:
            // 可在此处添加磁链结果处理（如存储、打印、用于控制）
            g_flux_identify.state = FLUX_IDLE;
            break;
    }
}

// ------------------- 限幅滤波器函数实现 -------------------
float LimitValue(float value, float min, float max)
	{
		if(value > max){
		return max;
		}else if(value < min){
		return min;
		}else{
		return value;
		}
}
// ------------------- 中值滤波器函数实现 -------------------

float MedianFilter(float new_value, float prev_values[3], uint8_t *idx)
	{
		prev_values[*idx] = new_value;
		*idx = (*idx + 1) % 3;
	//排序
	float temp[3] = {prev_values[0], prev_values[1], prev_values[2]};
	for(int i = 0;i < 2; i++){
			for(int j = 0;j < 2; j++){
				if (temp[j] > temp[j+1])
				{float tmp = temp[j];
				temp[j] = temp[j+1];
				temp[j+1] = tmp;
					
				
				}
			}
	}
	
		return temp[1];
}
// ------------------- 移动平均滤波函数实现 -------------------	

float MovingAverageFilter(float new_value, float *prev_value)
{
		float alpha = 0.2f;
		return alpha * new_value + (1 - alpha) * *prev_value;
	
}


// ------------------- 二阶带通滤波器函数实现 -------------------

void InitBiquadFilter(BiquadFilter *filter, float center_freq, float sample_freq, float q_factor){
	float omega = 2.0f * PI * center_freq / sample_freq;
	float sn = sin(omega);
	float cs = cos(omega);
	float alpha = sn / (2.0f * q_factor);
	
	filter->b0 = alpha;
	filter->b1 = 0.0f;
	filter->b2 = -alpha;
	filter->a1 = -2.0f * cs;
	filter->a2 = 1.0f - alpha;
	filter->x1 = filter->x2 = 0.0f;
	filter->y1 = filter->y2 = 0.0f;	
}

float ApplyBiquadFilter(BiquadFilter *filter, float input) {
    float output = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2
                  - filter->a1 * filter->y1 - filter->a2 * filter->y2;
    
    filter->x2 = filter->x1;
    filter->x1 = input;
    filter->y2 = filter->y1;
    filter->y1 = output;
    
    return output;
}





// 启动阶跃电感辨识（表贴式电机仅需辨识d轴）
void StepInductanceIdentifyStart(void) {
    if (g_step_inductance_identify.state == STEP_INDUCTANCE_IDLE) {
        // 重置参数
        MotorIdentifyInit();
        
        // 启动辨识
        g_step_inductance_identify.start_flag = true;
    }
}





// ------------------- 对外控制接口 -------------------
void ResistanceIdentifyStart(void)
{
    if (g_res_identify.state == RESISTANCE_IDLE) {
        g_res_identify.start_flag = true;
        MotorIdentifyInit(); // 重置电阻辨识状态
    }
}

void HighFreqInductanceStart(void)
{
    if (g_hf_inj.state == HF_IDLE) {
        // 确保电阻辨识已完成
        if (g_res_identify.state != RESISTANCE_FINISH) {
            ResistanceIdentifyStart();
            return;
        }
        
        // 检查电机转速是否适合高频注入
        if (abs(Encoder.ElectricalSpeed) < 5.0f) {
            // 电机转速过低，可选择低速注入模式或提示用户
            // 此处选择提示用户
//            Error_Handler("Motor speed too low for HF injection");
            return;
        }
        
        g_hf_inj.start_flag = true;
        g_hf_inj.Vh = 0.5f;          // 可动态配置
        g_hf_inj.wh = 2 * PI * 1000;  // 提高注入频率
        g_hf_inj.Ts = 1.0f / 10000;  // 可动态配置
        g_hf_inj.inj_idx = 0;
        g_hf_inj.cap_idx = 0;	
        g_hf_inj.u_dh = 0.0f;
        g_hf_inj.u_qh = 0.0f;		
		
        for (uint16_t i = 0; i < 256; i++) {
            g_hf_inj.i_dh_buf[i] = 0.0f;
            g_hf_inj.i_qh_buf[i] = 0.0f;
        }
        
        // 重新初始化带通滤波器，确保中心频率与注入频率匹配
        InitBiquadFilter(&i_d_bpf, 1000.0f, 10000.0f, 0.707f);
        InitBiquadFilter(&i_q_bpf, 1000.0f, 10000.0f, 0.707f);
    }
}


//// 在FOC控制中使用辨识得到的电感参数
//void FOCControllerInit(void) {
//    // 等待阶跃电感辨识完成
//    if (g_step_inductance_identify.state == STEP_INDUCTANCE_FINISH) {
//        // 获取辨识得到的电感值（表贴式电机Ld = Lq）
//        float L = g_step_inductance_identify.L_identified;
//        
//        // 配置电流环PI参数
//        CurrentController.Kp_d = L * CurrentLoopBandwidth;
//        CurrentController.Ki_d = g_res_identify.r_identified / CurrentLoopBandwidth;
//        CurrentController.Kp_q = L * CurrentLoopBandwidth;
//        CurrentController.Ki_q = g_res_identify.r_identified / CurrentLoopBandwidth;
//    }
//}


// ------------------- 中断/主循环调用 -------------------
void MotorIdentifyPeriodicTask(void)
{
    // 周期性调用（建议1ms周期）
    if (g_res_identify.start_flag) {
        ResistanceIdentifyProcess();
    }
    if (g_step_inductance_identify.start_flag) {
        StepInductanceIdentifyProcess();
    }		
		
		
    if (g_hf_inj.start_flag) {
//				HighFreqInductanceStart();
        HighFreqInductanceProcess();
    }
		if (g_flux_identify.start_flag) {
        FluxIdentifyProcess();
    }
}